
import serial
import struct
import time

class RH56HandController:
    def __init__(self, port, hand_id=1, baudrate=115200):
        # 初始化串口通信
        self.ser = serial.Serial(
            port=port,
            baudrate=baudrate,
            bytesize=serial.EIGHTBITS,
            parity=serial.PARITY_NONE,
            stopbits=serial.STOPBITS_ONE,
            timeout=1
        )
        self.hand_id = hand_id  # 手臂ID，默认为1

    def _calculate_checksum(self, data):
        # 计算校验和
        return sum(data) & 0xFF

    def _send_command(self, command_data):
        # 发送命令并等待响应
        self.ser.write(command_data)
        time.sleep(0.05)  # 等待设备响应
        response = self.ser.read_all()
        return response

    def read_register(self, address, length):
        # 构造读指令帧
        command = bytearray()
        command.extend([0xEB, 0x90, self.hand_id, 0x04, 0x11])  # 帧头
        address_low = address & 0xFF
        address_high = (address >> 8) & 0xFF
        command.extend([address_low, address_high, length])  # 地址和长度
        checksum = self._calculate_checksum(command[2:])  # 计算校验和
        command.append(checksum)  # 添加校验和
        
        response = self._send_command(command)
        if len(response) < 5 + length:
            raise ValueError("Invalid response length")  # 检查响应长度
        
        # 解析回复帧（示例：读取角度实际值）
        data_start = 7
        data = response[data_start : data_start + length]
        values = []
        for i in range(0, length, 2):
            value = struct.unpack('<H', data[i:i+2])[0]
            values.append(value)
        return values

    def write_register(self, address, data_values):
        # 构造写指令帧
        data_bytes = bytearray()
        for value in data_values:
            data_bytes.extend(struct.pack('<H', value))
        data_length = len(data_bytes)
        
        command = bytearray()
        command.extend([0xEB, 0x90, self.hand_id, data_length + 3, 0x12])  # 帧头
        address_low = address & 0xFF
        address_high = (address >> 8) & 0xFF
        command.extend([address_low, address_high])  # 地址
        command.extend(data_bytes)  # 数据
        checksum = self._calculate_checksum(command[2:])  # 计算校验和
        command.append(checksum)  # 添加校验和
        
        response = self._send_command(command)
        return response  # 可进一步解析确认是否写入成功

    def get_actual_angles(self):
        """读取各自由度实际角度值（寄存器ANGLE_ACT(m)地址1546）"""
        return self.read_register(0x060A, 12)  # 0x060A = 1546

    def set_target_angles(self, angles):
        """设置目标角度（寄存器ANGLE_SET(m)地址1486）"""
        if len(angles) != 6:
            raise ValueError("需要6个角度值（每个自由度一个）")
        return self.write_register(0x05CE, angles)  # 0x05CE = 1486

    def close(self):
        # 关闭串口通信
        self.ser.close()

if __name__ == "__main__":
    controller1 = RH56HandController(port="/dev/ttyUSB1")  # you shou
    controller2 = RH56HandController(port="/dev/ttyUSB2")  # zuoshou
    # try:
        # while True:
            # 读取实际角度
    # actual_angles1 = controller1.get_actual_angles()
    # print(f"1实际角度值: {actual_angles1}")
    actual_angles2 = controller2.get_actual_angles()
    print(f"2实际角度值: {actual_angles2}")
    # 设置目标角度（示例：设置各角度为100,100,100,100,2000,0）
    target_angles = [500, 950, 950, 950, 950, 950]
    controller1.set_target_angles(target_angles)
    controller2.set_target_angles(target_angles)
    print("目标角度已设置")
    time.sleep(2)
    target_angles = [200, 200, 200, 200, 500, 800]
    controller1.set_target_angles(target_angles)
    controller2.set_target_angles(target_angles)
    print("目标角度已设置")
    time.sleep(2)
        
    # finally:
    #     controller1.close()  # 确保在结束时关闭串口
    #     # controller2.close()  # 确保在结束时关闭串口
